#include <iostream>
#include <string.h>
#include <vector>
#include <stdexcept>
using namespace std;
//定义行列
const int ROW       =  20;
const int COL         =  20;

const int  Straight_linecost        =10;//直线代价Straight and diagonal lines
const int  Diagonal_linecost      =14;//斜线代价

//定义点
struct Pos
{
   int row,col;//点的坐标坐标
//    Pos();
//    Pos(int m_row, int m_col):row(m_row),col(m_col)
//    {};
};

//定义点和代价
struct Point
{
   struct Pos pos;
   int f,g,h;  //点的代价，f:最终代价，g:从起点到当前点的代价；h:当前点到终点的代价（不考虑障碍物）
};

//
enum direction {up,down,left,right,l_up,l_down,r_up,r_down};

//树节点定义
struct treeNode
{
   struct Point point;
   vector<treeNode *> sonNode;
   struct treeNode * parantNode;
};

treeNode * CreatTreeNode(int row,int col)
{
    treeNode * root = new treeNode ;
    memset(root,0,sizeof(treeNode));
    root->point.pos.row = row;
    root->point.pos.col   = col;
    return root;
}

int getH(Pos cur,Pos end)
{
    int dist_x = abs(end.col-cur.col)*10;
    int dist_y = abs(end.row-cur.row)*10;
    return (dist_x+dist_y);
}

int map[ROW][COL]={
        0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,   
        0,0,0,1,1,    1,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    
        0,0,0,1,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    
        0,1,1,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,  
        0,1,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,  

        0,1,1,1,1,    0,1,1,1,1,    1,1,1,1,1,    1,1,1,1,0,   
        0,1,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    
        0,1,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    
        0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,  
        0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,  

        0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,   
        0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    
        0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    
        0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,  
        0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,  
          
        0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,   
        0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    
        0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    
        0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,  
        0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0,    0,0,0,0,0
};

int passed_map[ROW][COL]={0};//相当于闭链表

int main(int argc, char  *argv[])
{
    struct Point  StartPoint,EndPoint;

    StartPoint.pos.row       = 2;
    StartPoint.pos.col         = 2;

    EndPoint.pos.row       =5;
    cout<<"设置错误，起始点是障碍物，无法到达！"<<endl;
    cout<<"设置错误，起始点是障碍物，无法到达！"<<endl;
    EndPoint.pos.col         = 4;

    if( map[StartPoint.pos.row][StartPoint.pos.col]==1)  //是不是障碍物，0空，1障碍物
    {
        cout<<"设置错误，起始点是障碍物，无法到达！"<<endl;
        return 0;
    }
    

    if(  map[EndPoint.pos.row][EndPoint.pos.col]==1)  //是不是障碍物，0空，1障碍物
    {
        cout<<"设置错误，终点是障碍物，无法到达！"<<endl;
        return 0;
    }
   
    // struct treeNode * rootNode = new treeNode;
    // rootNode->parantNode  = nullptr;
    // rootNode->point = StartPoint;

    //准备一棵树
    treeNode * pRoot = CreatTreeNode(StartPoint.pos.row,StartPoint.pos.col);
    //标记已经走过的点
    passed_map[StartPoint.pos.row][StartPoint.pos.col] = 1;
    //从起点开始
    treeNode * pCurrent = pRoot;
    treeNode * pChild = nullptr;

    vector< struct treeNode * > buff;//用于存储周围的多个点，相当于开链表，最终用于判断是否没有找到路径
    vector< struct treeNode * > ::iterator  it;
    vector< struct treeNode * > ::iterator  it_min;
    while(1)
    { 
        //循环寻路
        //1.找出当前节点周围能走的8个点
        for(int i=0;i<8;i++)
        {
            pChild = CreatTreeNode(pCurrent->point.pos.row,pCurrent->point.pos.col);
            switch (i)
            {
                case  up:
                    pChild->point.pos.row --;
                    pChild->point.g += 10;
                    break;
                case  down:
                    pChild->point.pos.row++;
                    pChild->point.g += 10;
                    break;
                case  direction::left:
                    pChild->point.pos.col--;
                    pChild->point.g += 10;
                    break;
                case  direction::right:
                    pChild->point.pos.col++;
                    pChild->point.g += 10;
                    break;
                case  l_up:
                    pChild->point.pos.row--;
                    pChild->point.pos.col--;
                    pChild->point.g += 14;
                    break;
                case  l_down:
                    pChild->point.pos.row++;
                    pChild->point.pos.col--;
                    pChild->point.g+= 14;
                    break;
                case  r_up:
                    pChild->point.pos.row--;
                    pChild->point.pos.col++;
                    pChild->point.g += 14;
                    break;
                case r_down:
                    pChild->point.pos.row++;
                    pChild->point.pos.col++;
                    pChild->point.g += 14;
                    break;
            }    //end of switch

              //判断子节点能不能走,空+没走过
            if( map[pChild->point.pos.row][pChild->point.pos.col]==0    //是不是障碍物，0空，1障碍物
                && 
                0==passed_map[pChild->point.pos.row][pChild->point.pos.col])    //判断是不是已经走过
                {
                //计算总代价
                pChild->point.h= getH(pChild->point.pos,EndPoint.pos);
                pChild->point.f= pChild->point.g+pChild->point.h;
                //2.入树  
                pCurrent->sonNode.push_back(pChild);
                pChild->parantNode =pCurrent;
                //入8个buff数组  
                buff.push_back(pChild);
                }
            else
                {
                delete pChild;
            } //end of if
        } //end of for

            it= buff.begin();
            it_min= buff.begin();

        //3.数组中挑出f最小的点
        for( ;it!=buff.end();it++)
        {
            it_min=(* it_min)->point.f<(* it)->point.f?it_min:it;

        }

        //4.走到下一步
        pCurrent= *it_min;
        passed_map[pCurrent->point.pos.row][pCurrent->point.pos.col] = 1;
        buff.erase(it_min);
        //5.遇到终点或整个图找遍了还没有找到终点  循环结束
        if(EndPoint.pos.row==pCurrent->point.pos.row
        &&
        EndPoint.pos.col==pCurrent->point.pos.col
        )
        {
            cout<<"found path"<<endl;
            //打印输出路径
            while (pCurrent)
            {
                cout<<"row"<<pCurrent->point.pos.row<<"   "<<"col"<<pCurrent->point.pos.col<<endl;
                pCurrent=pCurrent->parantNode;
            }
            cout<<endl;
            break;
        }

        if(buff.size()==0        )
        {
            cout<<"fail to find path"<<endl;
            break;
        }
        
    }
    return 0;
}
